#include <math.h>
#include <chrono>
#include <iostream>
#include <string>
#include <thread>
#include <vector>
#include <mutex>

#include "Eigen-3.3/Eigen/Core"
#include "Eigen-3.3/Eigen/QR"
#include "helpers.h"
#include "MPC.h"

#include <tf2/transform_datatypes.h>
#include <tf2/utils.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <tf2_eigen/tf2_eigen.h>
#include <lifecycle_msgs/msg/transition.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <rclcpp_lifecycle/lifecycle_publisher.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <nav_msgs/msg/path.hpp>

// for convenience
using std::string;
using std::vector;

class Semaphore
{
public:
  Semaphore(int count = 0) : count_(count) {}

  void notify()
  {
    std::unique_lock<std::mutex> lock(mutex_);
    count_++;
    cv_.notify_one();
  }

  void wait()
  {
    std::unique_lock<std::mutex> lock(mutex_);
    while (count_ == 0)
    {
      cv_.wait(lock);
    }
    count_--;
  }

private:
  std::mutex mutex_;
  std::condition_variable cv_;
  int count_;
};

// For converting back and forth between radians and degrees.
constexpr double pi() { return M_PI; }
double deg2rad(double x) { return x * pi() / 180; }
double rad2deg(double x) { return x * 180 / pi(); }
rclcpp::Node::SharedPtr _ros2_node;
std::string global_frame_id_ = "map";
bool initialpose_recieved_ = false;
Semaphore pose_update_sem_;
geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr current_pose_with_cov_stamped_ptr_ = nullptr;
sensor_msgs::msg::Imu::SharedPtr imu_ptr_ = nullptr;
nav_msgs::msg::Odometry::SharedPtr  odom_ptr_ = nullptr;
rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_pub_ = nullptr;
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr mpc_path_pub_ = nullptr;

void initialPoseReceived(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
{
  RCLCPP_INFO(_ros2_node->get_logger(), "initial PoseR eceived");
  if (msg->header.frame_id != global_frame_id_)
  {
    RCLCPP_WARN(_ros2_node->get_logger(), "initialpose_frame_id does not match global_frame_id");
    return;
  }
  initialpose_recieved_ = true;
}

void goalPoseReceived(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
{
  RCLCPP_INFO(_ros2_node->get_logger(), "goal Pose Received");
  if (msg->header.frame_id != global_frame_id_)
  {
    RCLCPP_WARN(_ros2_node->get_logger(), "goalpose_frame_id does not match global_frame_id");
    return;
  }
}

void poseReceived(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
{
  current_pose_with_cov_stamped_ptr_ = msg;
  pose_update_sem_.notify();
}

void imuReceived(const sensor_msgs::msg::Imu::SharedPtr msg)
{
  imu_ptr_ = msg;
}

void odomReceived(const nav_msgs::msg::Odometry::SharedPtr msg)
{
  odom_ptr_ = msg;
}

void thread_main_loop()
{
  vector<double> ptsx_ros;
  vector<double> ptsy_ros;
  // MPC is initialized here!
  MPC mpc;
  rclcpp::WallRate loop(100);

  while (rclcpp::ok())
  {
    // "42" at the start of the message means there's a websocket message event.
    // The 4 signifies a websocket message
    // The 2 signifies a websocket event
    pose_update_sem_.wait();
    if (nullptr != current_pose_with_cov_stamped_ptr_ && nullptr != imu_ptr_)
    {
      // j[1] is the data JSON object
      // vector<double> ptsx = j[1]["ptsx"];
      // vector<double> ptsy = j[1]["ptsy"];
      // double px = j[1]["x"];
      // double py = j[1]["y"];
      // double psi = j[1]["psi"];
      // double v = j[1]["speed"];

      float qw = imu_ptr_->orientation.w;
      float qx = imu_ptr_->orientation.x;
      float qy = imu_ptr_->orientation.y;
      float qz = imu_ptr_->orientation.z;

      float _q[4] = {qw, qx, qy, qz};
      float q_roll = atan2f(2.0f * (_q[0] * _q[1] + _q[2] * _q[3]), 1.0f - 2.0f * (_q[1] * _q[1] + _q[2] * _q[2]));
      float q_pitch = asinf(2.0f * (_q[0] * _q[2] - _q[3] * _q[1]));
      float q_yaw = atan2f(2.0f * (_q[0] * _q[3] + _q[1] * _q[2]), 1.0f - 2.0f * (_q[2] * _q[2] + _q[3] * _q[3]));

      double vx = odom_ptr_->twist.twist.linear.x; // cur velocity
      double vy = odom_ptr_->twist.twist.linear.y; // cur velocity

      vector<double> ptsx = ptsx_ros;                                       // nav planning points x
      vector<double> ptsy = ptsy_ros;                                       // nav planning points y
      double px = current_pose_with_cov_stamped_ptr_->pose.pose.position.x; // cur world position x
      double py = current_pose_with_cov_stamped_ptr_->pose.pose.position.y; // cur world position y
      double psi = q_yaw;                                                   // 当前航向角
      double v = vx / cos(q_yaw);

      assert(ptsx.size() == ptsy.size());

      // 转化世界坐标系航点为车辆坐标系航点
      vector<double> waypoints_x;
      vector<double> waypoints_y;
      for (int i = 0; i < ptsx.size(); i++)
      {
        double dx = ptsx[i] - px;
        double dy = ptsy[i] - py;
        waypoints_x.push_back(dx * cos(-psi) - dy * sin(-psi));
        waypoints_y.push_back(dx * sin(-psi) + dy * cos(-psi));
      }
      // 将指针赋值地址
      double *ptrx = &waypoints_x[0];
      double *ptry = &waypoints_y[0];
      // 使用指针来构建矩阵或向量，map是一个引用
      Eigen::Map<Eigen::VectorXd> waypoints_mx(ptrx, 6);
      Eigen::Map<Eigen::VectorXd> waypoints_my(ptry, 6);

      auto coeffs = polyfit(waypoints_mx, waypoints_my, 3);

      double cte = polyeval(coeffs, 0); // 当帧车辆行驶偏移量,px=0,py=0
      double epsi = -atan(coeffs[1]);   // 当帧航向角,psi = 0

      Eigen::VectorXd state(6);
      state << 0, 0, 0, v, cte, epsi; // px,py,psi都为0
      std::cout << "state: " << std::endl
                << state << std::endl;
      auto next_state = mpc.Solve(state, coeffs);

      /**
       * TODO: Calculate steering angle and throttle using MPC.
       * Both are in between [-1, 1].
       */
      double steer_value;
      double throttle_value;
      double velocity_value;

      steer_value = next_state[0];
      throttle_value = next_state[1];
      velocity_value = next_state[2];

      // NOTE: Remember to divide by deg2rad(25) before you send the
      //   steering value back. Otherwise the values will be in between
      //   [-deg2rad(25), deg2rad(25] instead of [-1, 1].
#if 0
          msgJson["steering_angle"] =  steer_value/(deg2rad(25)); //
          msgJson["throttle"] = throttle_value;
#endif

      // Display the MPC predicted trajectory
      vector<double> mpc_x_vals;
      vector<double> mpc_y_vals;
      nav_msgs::msg::Path mpc_path;

      /**
       * TODO: add (x,y) points to list here, points are in reference to
       *   the vehicle's coordinate system the points in the simulator are
       *   connected by a Green line
       */
      for (int i = 2; i < next_state.size(); i++)
      {
        geometry_msgs::msg::PoseStamped vehicle_pose;
        vehicle_pose.pose.position.z = 0;
        vehicle_pose.pose.orientation.x = 0;
        vehicle_pose.pose.orientation.y = 0;
        vehicle_pose.pose.orientation.z = 0;
        vehicle_pose.pose.orientation.w = 1;

        if (i % 2 == 0)
        {
          mpc_x_vals.push_back(next_state[i]);
          vehicle_pose.pose.position.x = next_state[i];
        }
        else
        {
          mpc_y_vals.push_back(next_state[i]);
          vehicle_pose.pose.position.y = next_state[i];
        }
        mpc_path.poses.push_back(vehicle_pose);
      }
      mpc_path.header.frame_id = "map";
      mpc_path.header.stamp = rclcpp::Clock().now();
      mpc_path_pub_->publish(mpc_path);

#if 0
          // Display the waypoints/reference line
          vector<double> next_x_vals;
          vector<double> next_y_vals;

          /**
           * TODO: add (x,y) points to list here, points are in reference to 
           *   the vehicle's coordinate system the points in the simulator are 
           *   connected by a Yellow line
           */
          for (double i = 0; i < 100; i += 3){
            next_x_vals.push_back(i);
            next_y_vals.push_back(polyeval(coeffs, i));
          }

          msgJson["next_x"] = next_x_vals;
          msgJson["next_y"] = next_y_vals;

          auto msg = "42[\"steer\"," + msgJson.dump() + "]";
          std::cout << msg << std::endl;
#endif
      // Latency
      // The purpose is to mimic real driving conditions where
      //   the car does actuate the commands instantly.
      //
      // Feel free to play around with this value but should be to drive
      //   around the track with 100ms latency.
      //
      // NOTE: REMEMBER TO SET THIS TO 100 MILLISECONDS BEFORE SUBMITTING.
#if 0
          std::this_thread::sleep_for(std::chrono::milliseconds(100));
          ws.send(msg.data(), msg.length(), uWS::OpCode::TEXT);
#endif
    } // end websocket if
    loop.sleep();
  }
}

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  _ros2_node = rclcpp::Node::make_shared("mpc");
  rclcpp::QoS qos(rclcpp::KeepLast{7});

  mpc_path_pub_ = _ros2_node->create_publisher<nav_msgs::msg::Path>("mpc/mpc_path", qos);

  auto initial_pose_sub_ = _ros2_node->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
      "initialpose", rclcpp::SystemDefaultsQoS(), &initialPoseReceived);

  auto goal_pose_sub_ = _ros2_node->create_subscription<geometry_msgs::msg::PoseStamped>(
      "goal_pose", rclcpp::SystemDefaultsQoS(), &goalPoseReceived);

  auto pose_sub_ = _ros2_node->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
      "pcl_pose", rclcpp::SystemDefaultsQoS(), &poseReceived);

  auto imu_sub_ = _ros2_node->create_subscription<sensor_msgs::msg::Imu>(
      "mobile_base/sensors/imu_data", qos, &imuReceived);

  auto odom_sub_ = _ros2_node->create_subscription<nav_msgs::msg::Odometry>(
      "odom", qos, &odomReceived);

  rclcpp::spin(_ros2_node);
  rclcpp::shutdown();
}